
#include "gskeleton.h"

GSkeleton::GSkeleton()
   :Skeleton() {
   
   int njoints = 20;
   closest_vertex.resize(njoints, -1);
   min_distance_vertex.resize(njoints, 10e8);
   onlyvisible.resize(njoints, true);
}

GSkeleton::GSkeleton(const Skeleton &s)
   :Skeleton(s) {
   
   limits(); // compute the center and radius of bounded sphere
   
   closest_vertex.resize(nBones()+1, -1);
   min_distance_vertex.resize(nBones()+1, 10e8);
   onlyvisible.resize(nBones()+1, true);
}

GSkeleton::~GSkeleton() {
   cout<<"   ::> ~GSkeleton()"<<endl;
}

Vector3 GSkeleton::getBoneProjector(const Point3 &v, const int idbone) {
   
   const Segment3 &bo = getBone(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   if (dotvaibiai<=0)                  { return va; }
   else if (dotvaibiai>=norma2aibi)    { return vb; }
   else {
      Number t = dotvaibiai/norma2aibi;
      return (Vector3(v[0],v[1],v[2]) - (Vector3(b[0],b[1],b[2])*t + Vector3(a[0],a[1],a[2])*(1.0-t)));
   }
}

float GSkeleton::getDistance(const Point3 &v, const int idbone, float t) {
   
   const Segment3 &bo = getBone(idbone);
   const Point3 &a = bo.source();
   const Point3 &b = bo.target();
   
   const Vector3 &ba = b-a;
   const Vector3 &va = v-a;
   const Vector3 &vb = v-b;
   
   const Number &norma2aibi = ba*ba;
   const Number &dotvaibiai = va*ba;
   
   Number dminvaibi;
   if (dotvaibiai<=0)                  { dminvaibi = sqrt(va*va); t=1.0;}
   else if (dotvaibiai>=norma2aibi)    { dminvaibi = sqrt(vb*vb); t=0.0;}
   else                                { 
      dminvaibi = sqrt(va*va - ((dotvaibiai*dotvaibiai)/norma2aibi)); 
      t = 1.0-dotvaibiai/norma2aibi;
   }
   
   return dminvaibi;
}

void GSkeleton::limits() {
   
   map<string, Bone>::iterator it = bones.begin();
   Number mincoo[3], maxcoo[3];
   Point3 p = point(it->second.getName());
   mincoo[0]=maxcoo[0]=p[0]; mincoo[1]=maxcoo[1]=p[1]; mincoo[2]=maxcoo[2]=p[2];
   while (it != bones.end()) {
      string b = it->second.getName();
      Point3 p = point(b);
      for (int i = 0; i < 3; ++i) {
         if (p[i] < mincoo[i])      mincoo[i] = p[i];
         else if (p[i] > maxcoo[i]) maxcoo[i] = p[i];
      }
      ++it;
   }
   
   Point3 WorldpMin(mincoo[0],mincoo[1],mincoo[2]);
   Point3 WorldpMax(maxcoo[0],maxcoo[1],maxcoo[2]);
   Vector3 size(WorldpMin, WorldpMax);
   size = size*0.5;
   _center = WorldpMin + size;
   _radius = sqrt(size * size); // vector lenght
}
